package com.dji.GSDemo.GaodeMap.Network;

import android.os.AsyncTask;
import android.util.Log;

import org.json.JSONArray;
import org.json.JSONObject;

import java.io.InputStream;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import java.net.Socket;
import java.net.SocketTimeoutException;
import java.util.ArrayList;
import java.util.List;

import dji.common.mission.waypoint.Waypoint;
import dji.common.mission.waypoint.WaypointAction;
import dji.common.mission.waypoint.WaypointActionType;

public class MonitorNetConnection {
    int CID = 0;
    int targetPort = 4396;
    String ipAddress = "10.201.83.45";

    boolean taskDone;
    boolean isServerReachable = true;

    public double lat = 181, lon = 181, altitude = 1;

    List<MonitorMessageListener> listeners = new ArrayList<>();

    private static class Singleton {
        private static final MonitorNetConnection instance = new MonitorNetConnection();
    }

    private MonitorNetConnection() {
    }

    public static MonitorNetConnection getInstance() {
        return Singleton.instance;
    }

    public int getCID() {
        return this.CID;
    }

    public void setIP(String ip) {
        this.ipAddress = ip;
    }

    public String getIpAddress() {
        return this.ipAddress;
    }

    public void setUAVInfo(double lat, double lon, double altitude) {
        this.lat = lat;
        this.lon = lon;
        this.altitude = altitude;
    }

    public boolean getServerReachable() {
        return isServerReachable;
    }

    public void setTaskDone(boolean taskDone) {
        this.taskDone = taskDone;
    }

    public void addListener(MonitorMessageListener obj) {
        if (listeners == null) {
            throw new NullPointerException();
        } else {
            if (!listeners.contains(obj)) {
                listeners.add(obj);
            }
        }
    }

    public void removeListener(MonitorMessageListener obj) {
        if (listeners == null) {
            throw new NullPointerException();
        } else {
            listeners.remove(obj);
        }
    }

    public void connectToServer() {
        AsyncTask.execute(new Runnable() {
            @Override
            public void run() {
                _connectToServer(); // 连接服务端
            }
        });
    }

    private void _connectToServer() {
        if (this.CID > 0) {
            return;
        }
        try {
            //主动发包
            JSONArray jsonarray = new JSONArray();
            JSONObject obj1 = new JSONObject();
            Object MAVCluster_Drone = "MAVCluster_Drone";
            obj1.put("Header", MAVCluster_Drone);
            obj1.put("Type", 0);
            JSONObject obj2 = new JSONObject();
            obj2.put("Lat", 0);
            obj2.put("Lon", 0);
            jsonarray.put(obj1);
            jsonarray.put(obj2);

            DatagramSocket s = sendMsgToMonitor(jsonarray.toString(), true);

            //收听回信
            final int DATA_LEN = 4096;
            byte[] inBuff = new byte[DATA_LEN];
            DatagramPacket inPacket = new DatagramPacket(inBuff, inBuff.length);
            s.setSoTimeout(5000);

            try {
                s.receive(inPacket);
                String msg = new String(inBuff, 0, inPacket.getLength());

                JSONArray result = new JSONArray(msg);
                JSONObject resultObject = (JSONObject) result.get(1);
                s.close();
                Object cidFromUDP = resultObject.get("CID");
                CID = (int) cidFromUDP;
                targetPort = 4396 + CID;
                startCommunication();

                for (MonitorMessageListener listener : listeners) {
                    listener.onServerMsgReceived(msg);
                }

                for (MonitorMessageListener listener : listeners) {
                    listener.onServerConnected(CID);
                }
            } catch (SocketTimeoutException e) {
                for (MonitorMessageListener listener : listeners) {
                    listener.log("Monitor unreachable");
                }
                Log.d("CoUAS Connection", "Monitor unreachable. " + e.getMessage());
            }

        } catch (Exception ex) {
            for (MonitorMessageListener listener : listeners) {
                listener.log("Monitor unreachable");
            }
            Log.e("CoUAS Connection", ex.getMessage());
        }
    }

    public void startCommunication() {
        new ReportToMonitorThread().start();
        new ListenToMonitorThread().start();
    }

    private class ReportToMonitorThread extends Thread {
        boolean keepRunning = true;

        @Override
        public void run() {
            reportToMonitor(); // 汇报状态服务端
        }

        public void stopThread() {
            keepRunning = false;
        }
    }

    private class ListenToMonitorThread extends Thread {
        boolean keepRunning = true;

        @Override
        public void run() {
            listenToMonitor(); // 从服务端监听命令
        }

        public void stopThread() {
            keepRunning = false;
        }
    }

    private void reportToMonitor() {
        try {
            int sendNum = 0;
            //如果飞机没有在执行任务
            while (true) {
                //如果飞机没有在执行任务，延迟0.5秒发包。
                long sleepTime = 500; //1000表示1秒。
                try {
                    sendNum = sendNum + 1;
                    Thread.sleep(sleepTime);
                    sendStateToMonitor(sendNum);
                } catch (Exception ex) {
                    // 键为null或使用json不支持的数字格式(NaN, infinities)
                    throw new RuntimeException(ex);
                }
            }
        } catch (Exception ex) {
            // 键为null或使用json不支持的数字格式(NaN, infinities)
            throw new RuntimeException(ex);
        }
    }

    private void sendStateToMonitor(int sendNum) {
        try {
            JSONArray jsonarray = new JSONArray();
            JSONObject obj1 = new JSONObject();
            Object MAVCluster_Drone = "MAVCluster_Drone";
            obj1.put("Header", MAVCluster_Drone);
            obj1.put("Type", 2);
            JSONObject obj2 = new JSONObject();
            obj2.put("CID", CID);
            obj2.put("Armed", 0);
            obj2.put("Mode", 0);
            obj2.put("Lat", lat);
            obj2.put("Lon", lon);
            obj2.put("Alt", altitude);
            jsonarray.put(obj1);
            jsonarray.put(obj2);

            sendMsgToMonitor(jsonarray.toString(), false);

        } catch (Exception ex) {
            throw new RuntimeException(ex);
        }
    }

    public void sendBDSMessageToMonitor(double lat, double lon) {
        try {
            JSONArray jsonarray = new JSONArray();
            JSONObject obj1 = new JSONObject();
            Object MAVCluster_Drone = "MAVCluster_Drone";
            obj1.put("Header", MAVCluster_Drone);
            obj1.put("Type", 7);
            JSONObject obj2 = new JSONObject();
            obj2.put("CID", CID);
            obj2.put("BDS_Lat", lat);
            obj2.put("BDS_Lon", lon);
            jsonarray.put(obj1);
            jsonarray.put(obj2);

            sendMsgToMonitor(jsonarray.toString(), false);

        } catch (Exception ex) {
            throw new RuntimeException(ex);
        }
    }

    public void sendArrivedMessageToMonitor(int step) {
        try {
            JSONArray jsonarray = new JSONArray();
            JSONObject msgHead = new JSONObject();
            Object MAVCluster_Drone = "MAVCluster_Drone";
            msgHead.put("Header", MAVCluster_Drone);
            msgHead.put("Type", MonitorMessageType.MAVC_ARRIVED.ordinal());
            JSONObject msgBody = new JSONObject();
            msgBody.put("CID", CID);
            msgBody.put("Step", step);
            jsonarray.put(msgHead);
            jsonarray.put(msgBody);

            sendMsgToMonitor(jsonarray.toString(), true);

        } catch (Exception ex) {
            throw new RuntimeException(ex);
        }
    }

    public void sendDoneMessageToMonitor(int step) {
        try {
            JSONArray jsonarray = new JSONArray();
            JSONObject msgHead = new JSONObject();
            Object MAVCluster_Drone = "MAVCluster_Drone";
            msgHead.put("Header", MAVCluster_Drone);
            msgHead.put("Type", MonitorMessageType.MAVC_DONE.ordinal());
            JSONObject msgBody = new JSONObject();
            msgBody.put("CID", CID);
            msgBody.put("Step", step);
            jsonarray.put(msgHead);
            jsonarray.put(msgBody);

            sendMsgToMonitor(jsonarray.toString(), true);

        } catch (Exception ex) {
            throw new RuntimeException(ex);
        }
    }

    private DatagramSocket sendMsgToMonitor(String msg, boolean enableListen) {
        try {
            byte[] bytes = msg.getBytes();
            DatagramSocket socket = new DatagramSocket();
            DatagramPacket outPacket = new DatagramPacket(bytes, bytes.length,
                    InetAddress.getByName(ipAddress), targetPort);
            socket.send(outPacket);

            if (enableListen) {
                for (MonitorMessageListener listener : listeners) {
                    listener.onSendMessage(msg);
                }
            }

            return socket;
        } catch (Exception ex) {
            throw new RuntimeException(ex);
        }
    }

    private void listenToMonitor() {
        try {
            Socket socketTCP = new Socket(ipAddress, targetPort);
            try {
                Thread.sleep(2000);
                socketTCP.setSoTimeout(5000);

                int whileNum = 0;
                String buf = "";
                while (!taskDone) {
                    whileNum = whileNum + 1;
                    InputStream din = socketTCP.getInputStream(); // 获得DataInputStream对象
                    isServerReachable = true;
                    byte[] streamTcp = new byte[2048];
                    int streamLength = 0;
                    try {
                        streamLength = din.read(streamTcp);
                    } catch (Exception ex) {
                        // 键为null或使用json不支持的数字格式(NaN, infinities)
                        return;
                    }

                    String msgTCP = new String(streamTcp, 0, streamLength);
                    buf = buf + msgTCP;
                    int msgLength = msgTCP.length();
                    boolean isEnd = msgTCP.substring(msgLength - 2).equals("$$");

                    if (!isEnd) {
                        continue;
                    }

                    String msg = buf.substring(0, buf.length() - 2);
                    buf = "";

                    JSONArray commandJson = new JSONArray(msg);
                    JSONObject commandHead = (JSONObject) commandJson.get(0);
                    boolean hasMAVCluster_Monitor = commandHead.get("Header").toString().equals("MAVCluster_Monitor");
                    if (hasMAVCluster_Monitor) {
                        String mavc_type_object = commandHead.get("Type").toString();
                        int mavc_type = Integer.parseInt(mavc_type_object);
                        new handleMsgThread(mavc_type, commandJson).start();
                    }
                }

            } catch (Exception ex) {
                if (ex instanceof SocketTimeoutException) {
                    socketTCP.close();
                    isServerReachable = false;
                    listenToMonitor();
                }
                throw new RuntimeException(ex);
            }
        } catch (Exception ex) {
            throw new RuntimeException(ex);
        }
    }

    private class handleMsgThread extends Thread {
        private JSONArray commandJson;
        private int mavc_type;


        public handleMsgThread(int mavc_type, JSONArray commandJson) {
            this.mavc_type = mavc_type;
            this.commandJson = commandJson;
        }

        @Override
        public void run() {
            if (this.mavc_type == 4) {
                handleMAVCActionJson(this.commandJson);
            }
        }
    }

    private void handleMAVCActionJson(JSONArray commandJson) {
        try {
            for (int i = 0; i < commandJson.length() - 1; i++) {
                JSONObject msgBody = (JSONObject) commandJson.get(i + 1);
                int msgCID = (int) msgBody.get("CID");
                if (msgCID == CID) {

                    MonitorActionType actionType = MonitorActionType.values()[(int) msgBody.get("Action_type")];
                    int step = msgBody.has("Step") && !msgBody.isNull("Step") ? (int) msgBody.get("Step") : 0;

                    switch (actionType) {
                        case ACTION_ARM_AND_TAKEOFF:
                            break;
                        case ACTION_GO_TO:
                            break;
                        case ACTION_GO_BY:
                            break;
                        case ACTION_LAND:
                            break;
                        case ACTION_EXEC_WAYPOINT_TASK: {
                            for (MonitorMessageListener listener : listeners) {
                                listener.onServerMsgReceived(commandJson.toString());
                            }
                            try {
                                List<Waypoint> waypoints = new ArrayList<>();
                                JSONArray points = (JSONArray) msgBody.get("Points");
                                for (int m = 0; m < points.length(); m++) {
                                    JSONObject point = (JSONObject) points.get(m);
                                    JSONObject pos = (JSONObject) point.get("Pos");
                                    double lat = (double) pos.get("Lat");
                                    double lon = (double) pos.get("Lon");
                                    double altitude = (double) pos.get("Alt");
                                    Waypoint waypoint = new Waypoint(lat, lon, (float) altitude);

                                    JSONObject Gimbal = (JSONObject) point.get("Gimbal");
                                    int yaw = (int) Gimbal.get("Yaw");
                                    int pitch = (int) Gimbal.get("Pitch");
                                    waypoint.heading = yaw;
                                    waypoint.addAction(new WaypointAction(WaypointActionType.GIMBAL_PITCH, pitch));
                                    waypoint.addAction(new WaypointAction(WaypointActionType.START_TAKE_PHOTO, 6));
                                    waypoint.addAction(new WaypointAction(WaypointActionType.STAY, 1));

                                    waypoints.add(waypoint);
                                }

                                for (MonitorMessageListener listener : listeners) {
                                    listener.onMsgExecWaypointTask(step, waypoints);
                                }
                            } catch (Exception e) {
                                listeners.get(0).log(e.getMessage());
                            }

                            break;
                        }
                        default:
                            break;
                    }
                }
            }
        } catch (Exception ex) {
            // 键为null或使用json不支持的数字格式(NaN, infinities)
            throw new RuntimeException(ex);
        }
    }
}

